import socket
from machine import PWM, Pin
import network
import time
import ujson
import _thread

from quad import Quad

# ######################################################
# 开始设置全局变量
# ######################################################
SSID = '小亦站'
PASSWORD = '88889999'
IFCONFIG = ("192.168.2.190", "255.255.255.0", "192.168.2.1", "8.8.8.8")
PORT_DATA = 12345

# PID 控制参数
Kp_dist = 0.02
Ki_dist = 0.001
Kd_dist = 0.002

Kp_angle = 0.2
Ki_angle = 0.01
Kd_angle = 0.02

# 完成任务条件
SUCCESS_ANGLE = 10
SUCCESS_DISTANCE = 100


# 初始化误差
previous_error_dist = 0
integral_dist = 0

previous_error_angle = 0
integral_angle = 0



# 实例化四足机器人
quad = Quad()
quad.init(12, 16, 25, 18, 13, 17, 26, 19)
quad.setTrims(0, 0, 0, 0, 0, 0, 0, 0)
# ######################################################
# 结束设置全局变量
# ######################################################


# 连wifi
wlan = network.WLAN(network.STA_IF)
wlan.ifconfig(IFCONFIG)
wlan.active(True)
if not wlan.isconnected():
    print('connecting to network...')
    wlan.connect(SSID, PASSWORD)
    i = 1
    while not wlan.isconnected():
        print("正在链接...{}".format(i))
        i += 1
        time.sleep(1)


print('Connecting to WiFi...', end='')
while not wlan.isconnected():
    time.sleep(0.5)
    print('.', end='')
print('\nConnection successful')
print(wlan.ifconfig())


def stop_quad():
    global previous_error_dist, integral_dist, previous_error_angle, integral_angle
    quad.home()
    previous_error_dist = 0
    integral_dist = 0
    previous_error_angle = 0
    integral_angle = 0
    print("stop_motor!")


def pid_control(angle, distance):
    global previous_error_dist, integral_dist, previous_error_angle, integral_angle

    # 计算距离误差
    error_dist = distance
    integral_dist += error_dist
    derivative_dist = error_dist - previous_error_dist
    previous_error_dist = error_dist

    # 计算角度误差
    error_angle = angle
    integral_angle += error_angle
    derivative_angle = error_angle - previous_error_angle
    previous_error_angle = error_angle

    # PID 控制计算
    control_signal_dist = Kp_dist * error_dist + Ki_dist * integral_dist + Kd_dist * derivative_dist
    control_signal_angle = Kp_angle * error_angle + Ki_angle * integral_angle + Kd_angle * derivative_angle

    # 计算左右轮速度
    print("control_signal_dist:", control_signal_dist)
    print("control_signal_angle:", control_signal_angle)
    left_speed = int(control_signal_dist + control_signal_angle)
    right_speed = int(control_signal_dist - control_signal_angle)

    return left_speed, right_speed


def limit_value(v, min_v, max_v):
    if v > max_v:
        return max_v
    elif v < min_v:
        return min_v
    else:
        return v

def handle_data_connection(conn_data):
    while True:
        data = conn_data.recv(1024)
        if not data:
            break
        try:
            command = data.decode().strip()

            # 没有检测到目标, 暂停电机
            if command == "0:0":
                stop_quad()
                conn_data.sendall(b'ACK')
                continue

            # 解析角度和距离数据
            angle, distance = map(float, command.split(':'))
            print(f"Received angle: {angle}, distance: {distance}")

            # 利用角度和距离数据, PID方式控制电机
            if abs(distance) > SUCCESS_DISTANCE:

                left_speed, right_speed = pid_control(angle, distance)
                print("left_speed:", left_speed)
                print("right_speed:", right_speed)
                left_speed_limit = limit_value(left_speed, -20, 20)
                right_speed_limit = limit_value(right_speed, -20, 20)
                print("left_speed_limit:", left_speed_limit)
                print("right_speed_limit:", right_speed_limit)
                quad.forward_pid(left_speed_limit, right_speed_limit, t=1000)

            else:
                stop_quad()
                print("到达目标点!")
                time.sleep(1)

            # 给客户端返回确认消息, 让客户端进入下一个循环, 不断发送新数据
            conn_data.sendall(b'ACK')

        except Exception as e:
            print(f"Error: {e}")
            stop_quad()
            conn_data.sendall(b'ACK')

    conn_data.close()


# 创建套接字, 控制机器人
sock_data = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock_data.bind(('', PORT_DATA))
sock_data.listen(1)
print(f'Socket data listening on port {PORT_DATA}')


if __name__ == '__main__':

    while True:
        conn_data, addr_data = sock_data.accept()
        print('Connected by data sender:', addr_data)
        handle_data_connection(conn_data)

